#######调配各关卡主程序########
#####支持库#######
import cv2
import math
import threading
import numpy as np
import time
#########关卡##########
import basic_game as bg
from demo_mine import mine #地雷关卡
from demo_step import step #台阶关卡
from demo_baffle import baffle #挡板关卡
from demo_single_bridge import bridge
from demo_pit import pit
#import demo_gate as gate#起始匝口关卡
#import demo_baffle as bf#跨栏关卡
import global_var as glv
import visual_function as vf

stream = "http://127.0.0.1:8080/?action=stream?dummy=param.mjpg"
cap = cv2.VideoCapture(stream)

####原图像信息####
resize_width = glv.getVal("resize_width")
resize_height = glv.getVal("resize_height")
#####标志位####
in_mission = False #是否正处于任务中
now_mission = None #当前任务种类
mission = None #当前的任务
running = True
#####变量######
color_range = glv.getColorRange(-1)
colorMission = glv.getColorMission()
MissionTable = glv.MissionTable
orgFrame = None
ret = False
level = 6
PWM_Y = 1900 #初始y轴云台角度
bg.main_init(PWM_Y)
maxCount = 8
count = 6

def get_image():
    global orgFrame
    global ret
    global cap
    global count
    global maxCount
    while True:
        if cap.isOpened():
            Ret, frameTemp = cap.read()
            if Ret:
                count -= 1
            else:
                time.sleep(0.01)
            if count == 0:
                count = maxCount
                ret = True
                orgFrame = frameTemp
th1 = threading.Thread(target = get_image)
th1.setDaemon(True)
th1.start()

def getNewMission(hsvImage):
    global colorMission
    color = vf.getColor(hsvImage)
    print(color)
    if color == None:
        #识别不到颜色，直走
        bg.go_oneStep()
        return None
    mission = colorMission[color]
    return mission

while True:
    if orgFrame is not None and ret:
        t1 = cv2.getTickCount()
        orgframe = cv2.resize(orgFrame, (resize_width,resize_height))
        outframe = orgframe.copy()
        orgframe = cv2.GaussianBlur(orgframe, (3,3), 0)
        hsvframe = cv2.cvtColor(orgframe,cv2.COLOR_BGR2HSV)
        h, s, v = cv2.split(hsvframe)
        v = cv2.equalizeHist(v)
        #合并三个通道
        hsvframe = cv2.merge((h,s,v))
        cv2.imshow('hsvframe',hsvframe)
        if in_mission:
            if mission is not None:
                if now_mission == "detection_gate":
                    getColor, missionTemp = mission.run(hsvframe, outframe)
                    if getColor:
                        now_mission = missionTemp
                        in_mission = False
                        mission = None
                else:
                    throughGate = mission.run(hsvframe, outframe)
                    if throughGate:
                        in_mission = False
                        mission = None
                        level += 1
                        #云台初始化
                        bg.main_init(PWM_Y)
            else:
                in_mission = False
        else:
            #获取到当前任务种类
            now_mission = MissionTable[level]
            #如果是需要识别颜色识别关卡的话，执行检测关卡程序
            if now_mission == "color_gate":
                in_mission = True
                now_mission = "detection_gate"
                mission = bg.NextColorGate(hsvframe)
                #print(now_mission)
            if now_mission == "single_gate":
                in_mission = True
            elif now_mission == "step":
                mission = step(running)
                in_mission = True
            elif now_mission == "mine":
                mission = mine(running)
                in_mission = True
            elif now_mission == "baffle":
                mission = baffle(running)
                in_mission = True
            elif now_mission == "turn":
                in_mission = True
            elif now_mission == "door":
                in_mission = True
            elif now_mission == "single_bridge":
                mission = bridge(running)
                in_mission = True
            elif now_mission == "football":
                in_mission = True
            elif now_mission == "pit":
                in_mission = True
                mission = pit(running)
            elif now_mission == "gully":
                in_mission = True
            elif now_mission == "double_gate":
                in_mission = True
        t2 = cv2.getTickCount()
        time_r = (t2 - t1)/cv2.getTickFrequency()
        fps = 1.0/time_r
        cv2.imshow("outframe",outframe)
        cv2.putText(outframe, "mission: " + str(now_mission),
        (10, resize_height - 55), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
    
        cv2.putText(outframe, "fps:" + str(int(fps)),
        (10, resize_height - 8), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
        cv2.waitKey(1)

        
    else:
        pass
